#include "precom.h"
#include "Path5C.h"
#include <fstream>

using namespace std;
using namespace fmath;

Path5CPoint::Path5CPoint()
{
	m_point.set(0.f, 0.f, 0.f);
	m_eulerAngles.set(0.f, 0.f);
}

Path5C::Path5C()
{
	m_stepDistance = 0.01f;
	/*m_iterNextKnot = m_path.begin();
	m_curPoint = *m_iterNextKnot;
	++m_iterNextKnot;*/
}

Path5C::Path5C(wchar_t* a_fileName)
{
	LoadFromFile(a_fileName);
}

Path5C::~Path5C()
{
	m_path.clear();
	m_iterNextKnot = m_path.begin();
}

int Path5C::LoadFromFile(wchar_t* a_fileName)
{
	Path5CPoint knot;
	fstream file;
	file.open(a_fileName, ios::in);
	if( !file.is_open() )
		return CANT_OPEN_FILE;
	m_path.clear();
	while( !file.eof() )
	{
		file >> knot.m_point.x;
		file >> knot.m_point.y;
		file >> knot.m_point.z;
		file >> knot.m_eulerAngles.x;
		file >> knot.m_eulerAngles.y;
		m_path.push_back(knot);
	}
	file.close();

	if(m_path.empty()) 
		return NO_NEED_FOR_ACTION;
	
	m_iterNextKnot = m_path.begin();
	m_curPoint = *m_iterNextKnot;
	m_prevPoint = *m_iterNextKnot;
	++m_iterNextKnot;
	return ALL_OK;
}

int Path5C::SaveToFile(wchar_t* a_fileName)
{
	list<Path5CPoint>::iterator iter = m_path.begin();
	fstream file;
	
	if( m_path.empty() ) return NO_NEED_FOR_ACTION;
	file.open(a_fileName, ios::out | ios::trunc);
	if( !file.is_open() )
		return CANT_OPEN_FILE;
	while( iter != m_path.end() )
	{
		file << iter->m_point.x << " ";
		file << iter->m_point.y << " ";
		file << iter->m_point.z << " ";
		file << iter->m_eulerAngles.x << " ";
		file << iter->m_eulerAngles.y << "\n";
		++iter;
	}
	file.close();
	return ALL_OK;
}

/*************************************************
 *  Oblicza polozenie nastepnego punktu, zwraca 0,
 * modyfikuje argument a_distance, ktory jesli jest
 * dodatni wyznacza dlugosc kroku wzdloz nastepnego 
 * odcinka sciezki, zas jesli ujemny - ile zostalo
 * do konca bierzacego odcinak sciezki
 */
int Path5C::CalculateNextPoint(float& a_distance)
{
	vector3 dir;
	vector2 ang;
	dir = m_iterNextKnot->m_point - m_curPoint.m_point;
	dir.normalize();
	ang = m_iterNextKnot->m_eulerAngles - m_curPoint.m_eulerAngles;
	ang.normalize();

	m_curPoint.m_point += a_distance * dir;
	m_curPoint.m_eulerAngles += a_distance * ang;
		
	//sprawdzenie czy nie wyszlismy poza odcinek
	dir = (m_curPoint.m_point - m_prevPoint.m_point);
	a_distance = dir.length();
	dir = (m_iterNextKnot->m_point - m_prevPoint.m_point);
	a_distance -= dir.length();

	return ALL_OK;
}

int Path5C::TakeStep()
{
	float dist = m_stepDistance;

	if( m_iterNextKnot == m_path.end() )
		return NO_NEED_FOR_ACTION;

	CalculateNextPoint(dist);
	while (dist > EPS) {
		m_curPoint = m_prevPoint = *m_iterNextKnot;
		++m_iterNextKnot;
		CalculateNextPoint(dist);
	}

	return ALL_OK;
}

int Path5C::TakeStep(float a_distance)
{
	float remember = m_stepDistance;
	TakeStep();
	m_stepDistance = remember;
	return ALL_OK;
}